package butines.common.entities;

import java.awt.Color;
import java.awt.Graphics2D;
import java.util.ArrayList;
import java.util.List;

import butines.common.math.Vector2D;
import butines.common.steering.ProximityDatabase;
import butines.common.steering.SteerLibrary;
import butines.common.steering.Vehicle;
import butines.common.steering.ProximityDatabase.Token;

public abstract class EntityMoving extends AbstractEntity implements Vehicle {

	private float maxForce = .15f;
	private float maxSpeed = 1f;
	private float mass = 1f;
	private float speed;
	
	private Vector2D velocity = new Vector2D();
	private Vector2D accel = new Vector2D();
	
	private Vector2D previousPosition = new Vector2D();
	private Vector2D predictFuturePosition = new Vector2D();
	
	private Token<EntityMoving> proximityToken;
	private List<EntityMoving> neighbors = new ArrayList<EntityMoving>();

	
	private Vector2D[] smoothedHeading;
	private int nextSmoothHeading;

	public void smoothing() {
		int len = smoothedHeading == null ? 12 : smoothedHeading.length;
		if (smoothedHeading == null) {
			smoothedHeading = new Vector2D[len];
			for (int i = 0; i < len; i++) {
				smoothedHeading[i] = new Vector2D();
			}
		}
		
		smoothedHeading[nextSmoothHeading].set(getForward());
		nextSmoothHeading = (nextSmoothHeading < smoothedHeading.length - 1) ? nextSmoothHeading + 1 : 0;

		getHeading().set(0, 0);
		for (int i = 0; i < len; i++) {
			getHeading().add(smoothedHeading[i]);
		}
		getHeading().div(len);
	}
		
		
	public EntityMoving() {
		
	}

	public void setProximityDatabase(ProximityDatabase<EntityMoving> proximityDatabase) {
		if (proximityToken != null) {
			proximityToken.release();
			proximityToken = null;
		}
		if (proximityDatabase != null) {
			proximityToken = proximityDatabase.allocateToken(this);
			proximityToken.notifyNewPosition(getPosition());
		}
	}
	
	public Vector2D getPredictFuturePosition() {
    	predictFuturePosition.set(getPosition());
    	predictFuturePosition.add(getVelocity());
		return predictFuturePosition;
	}

	public Vector2D getPreviousPosition() {
		return previousPosition;
	}

	public void setSpeed(float speed) {
		if (speed == 0) {
			this.speed = 0;
			this.velocity.set(0, 0);
		} else if (speed > 0) {
			this.speed = speed;
			this.velocity.mult(getForward(), Math.min(speed, getMaxSpeed()));
		} else {
			throw new IllegalArgumentException("speed is negative");
		}
	}

	public float getSpeed() {
		return speed;
	}

	private void enforceNonPenetrationContraint(AbstractEntity obstacle) {
		if (this == obstacle || obstacle == null || !isAlive() || !isSolid()) {
			return;
		}
		if (obstacle.isAlive() && obstacle.isSolid()) {
			float minDistance = getRadius() + obstacle.getRadius();
			if (SteerLibrary.enforceNonPenetrationConstraint(this, obstacle, minDistance)) {
				onCollide(obstacle);
			}
		}
	}
	
	protected void onCollide(AbstractEntity obstacle) {
		// override
	}
	
	public void applySteeringForce(Vector2D steeringForce) {
		previousPosition.set(getPosition());
		
		accel.set(steeringForce);
		accel.div(getMass());
		Vector2D.truncate(accel, getMaxForce());
		
		velocity.add(accel);
		Vector2D.truncate(velocity, getMaxSpeed());
		
		getPosition().add(velocity);

		speed = velocity.length();
		
		if (speed > 0) {
			getForward().normalise(velocity);
			getSide().set(-getForward().y, getForward().x);
		}
		
		smoothing();
		
		findNeighbors(getPosition(), getViewDistance());
		for (AbstractEntity obstacle: neighbors) {
			enforceNonPenetrationContraint(obstacle);
		}
		notifyNewPosition();
	}
		
	public void applyBrakingForce(float rate) {
		velocity.mult(rate);
		speed = velocity.length();
	}

	public void release() {
		if (proximityToken != null) {
			proximityToken.release();
			proximityToken = null;
		}
	}
	
	private void notifyNewPosition() {
		if (proximityToken != null) {
			proximityToken.notifyNewPosition(getPosition());
		}
	}
	
	
	public void findNeighbors(Vector2D center, float radius) {
		if (proximityToken != null) {
			proximityToken.findNeighbors(center, radius, neighbors);
		}
	}
	
	public List<EntityMoving> getNeighbors() {
		return neighbors;
	}

	@Override
	public void draw(Graphics2D g2d) {
		int x = Math.round(getPosition().x-getRadius());
		int y = Math.round(getPosition().y-getRadius());
		int w = Math.round(getRadius() * 2);
		g2d.setColor(Color.YELLOW);
		g2d.drawOval(x, y, w, w);
	}

	public float getMass() {
		return mass;
	}

	public void setMass(float mass) {
		this.mass = mass;
	}

	public float getMaxForce() {
		return maxForce;
	}

	public void setMaxForce(float maxForce) {
		this.maxForce = maxForce;
	}

	public float getMaxSpeed() {
		return maxSpeed;
	}

	public void setMaxSpeed(float maxSpeed) {
		this.maxSpeed = maxSpeed;
	}

	public Vector2D getVelocity() {
		return velocity;
	}

	public Vector2D getAccel() {
		return accel;
	}

}
